/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * command.cxx
 *
 * PURPOSE:
 *  This code implements the command data structure between tasks on 
 *  the flight computer and from ground control station
 *
 * CREATED:
 * 11/2000 by Cory Dixon
 *
 * LAST MODIFIED:
 * $Author: dixonc $
 * $Date: 2005/09/09 19:35:44 $
 * $Revision: 1.8 $
 *
 ***********************************************************************/

#include <stdio.h>
#include <string.h>
#include <assert.h>
#include <iostream>

#include "command.h"

// these are used for cout
using std::hex;
using std::dec;

//////////////////////////////////////////////////////////////////////
// Defined Commands 
//////////////////////////////////////////////////////////////////////
const DefinedCommandStruct DefinedCommands[CMD_MAX_NUM] = {
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},  

	{CMD_JOYSTICK,        0,                          "CMD_JOYSTICK"       ,        CMD_COMMAND}, // 0x10
	{CMD_SERVO,           sizeof(Servos_t),           "CMD_SERVOS"         ,    CMD_DATA_STREAM},
	{CMD_BUTTONS,         sizeof(Buttons_t),"CMD_BUTTONS"                   ,        CMD_COMMAND},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{CMD_IMU,             sizeof(IMU_t),              "CMD_IMU"            ,    CMD_DATA_STREAM}, // 0x20
	{CMD_IMU_ANG,         sizeof(IMUAngles_t),        "CMD_IMU_ANG"        ,    CMD_DATA_STREAM},
	{CMD_IMU_RATE,        sizeof(IMURates_t),         "CMD_IMU_RATE"       ,    CMD_DATA_STREAM},
	{CMD_IMU_ACC,         sizeof(IMUAccelerations_t), "CMD_IMU_ACC"        ,    CMD_DATA_STREAM},
	{CMD_MAG_TELEM,       sizeof(MagStruct_t),        "CMD_MAG_TELEM"      ,    CMD_DATA_STREAM},
	{CMD_RSSI_TELEM,      sizeof(RSSIStruct_t),       "CMD_RSSI_TELEM"     ,    CMD_DATA_STREAM},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{CMD_AIR_DATA,        0,                          "CMD_AIR_DATA"       ,        CMD_NO_TYPE}, // 0x30
	{CMD_WIND,            sizeof(WindData_t),         "CMD_WIND"           ,    CMD_DATA_STREAM},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{CMD_GPS,             0,                          "CMD_GPS"            ,        CMD_NO_TYPE}, // 0x40
	{CMD_GPS_LATLON,      sizeof(GPSLatLon_t),        "CMD_GPS_LATLON"     ,    CMD_DATA_STREAM},
	{CMD_GPS_ALT,         sizeof(GPSAlt_t),           "CMD_GPS_ALT"        ,    CMD_DATA_STREAM},
	{CMD_GPS_UTC,         sizeof(GPSUTC_t),           "CMD_GPS_UTC"        ,    CMD_DATA_STREAM},
	{CMD_GPS_TRACK,       sizeof(GPSTrack_t),         "CMD_GPS_TRACK"      ,    CMD_DATA_STREAM},
	{CMD_GPS_SOG,         sizeof(GPSSOG_t),           "CMD_GPS_SOG"        ,    CMD_DATA_STREAM},
	{CMD_GPS_ACCURACY,    sizeof(GPSAccuracy_t),      "CMD_GPS_ACCURACY"   ,    CMD_DATA_STREAM},
	{CMD_GPS_TELEM,       sizeof(GPSTelem_t),         "CMD_GPS_TELEM"      ,    CMD_DATA_STREAM},
	{CMD_GPS_LLA,         sizeof(GPSLLA_t),           "CMD_GPS_LLA"      ,      CMD_DATA_STREAM},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{CMD_COMM,            0,                          "CMD_COMM"           ,        CMD_NO_TYPE}, // 0x50
	{CMD_LINK_STATUS,     CMD_DYNAMIC_SIZE,           "CMD_LINK_STATUS"    ,    CMD_DATA_STREAM},
	{CMD_RATE_TEST,       sizeof(RateTest_t),         "CMD_RATE_TEST"      ,        CMD_COMMAND},
	{CMD_CHAT,            CMD_DYNAMIC_SIZE,           "CMD_CHAT"           ,CMD_DATA_STREAM},
	{CMD_CONNECT,         sizeof(CommStruct_t),      "CMD_CONNECT"        ,        CMD_COMMAND},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{CMD_PAYLOAD,         0,                          "CMD_PAYLOAD"        ,        CMD_NO_TYPE}, // 0x60
	{CMD_PTH,             sizeof(PTH_t),              "CMD_PTH"            ,    CMD_DATA_STREAM},
	{CMD_SONDE,           sizeof(Sonde_t),            "CMD_SONDE"          ,    CMD_DATA_STREAM},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{CMD_H_AND_S,         0,                          "CMD_H_AND_S"        ,        CMD_NO_TYPE}, // 0x70
	{CMD_ROOMBA_STATUS,                 sizeof(SCISensorPacket_t),                          "CMD_ROOMBA_STATUS"                   ,        CMD_DATA_STREAM},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{CMD_AUTOPILOT,       0,                          "CMD_AUTOPILOT"      ,        CMD_NO_TYPE}, // 0x80
	{CMD_GOTO_WP,         sizeof(GotoWP_t),           "CMD_GOTO_WP"        ,        CMD_COMMAND},
	{CMD_FP,              CMD_DYNAMIC_SIZE,           "CMD_FP"             ,        CMD_COMMAND},
	{CMD_TELEM,           sizeof(Telemetry_t),        "CMD_TELEM"          ,    CMD_DATA_STREAM},
	{CMD_AP_RATES,        sizeof(APRates_t),          "CMD_AP_RATES"       ,        CMD_COMMAND},
	{CMD_FP_SUBS,         CMD_DYNAMIC_SIZE,           "CMD_FP_SUBS"        ,    CMD_DATA_STREAM},
	{CMD_ALT_LOOP,        sizeof(GotoAlt_t),          "CMD_ALT_LOOP"       ,        CMD_COMMAND},
	{CMD_IAS_LOOP,        sizeof(GotoIAS_t),          "CMD_IAS_LOOP"       ,        CMD_COMMAND},
	{CMD_BANK_LOOP,       sizeof(GotoBank_t),         "CMD_BANK_LOOP"      ,        CMD_COMMAND},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{CMD_FLIGHT_COMPUTER, 0,                          "CMD_FLIGHT_COMPUTER",        CMD_NO_TYPE}, // 0x90
	{CMD_ACK,             1,                          "CMD_ACK"            ,       CMD_ACK_NACK},
	{CMD_FAIL,            1,                          "CMD_FAIL"           ,       CMD_ACK_NACK},
	{CMD_COORDINATE,      sizeof(Coordinate_t),       "CMD_COORDINATE"     ,        CMD_COMMAND},
	{CMD_CUSTODY,         sizeof(Custody_t),          "CMD_CUSTODY"        ,        CMD_COMMAND},
	{CMD_TRACK,           CMD_DYNAMIC_SIZE,           "CMD_TRACK"          ,        CMD_COMMAND},
	{CMD_PHASE,           sizeof(Phase_t),            "CMD_PHASE"          ,        CMD_COMMAND},
	{CMD_SEARCH,          0,                          "CMD_SEARCH"         ,        CMD_COMMAND},
	{CMD_RETURN,          0,                          "CMD_RETURN"         ,        CMD_COMMAND},
	{CMD_FORMATION,       CMD_DYNAMIC_SIZE,           "CMD_FORMATION"      ,        CMD_COMMAND},
	{CMD_CHAIN,           CMD_DYNAMIC_SIZE,           "CMD_CHAIN"          ,        CMD_COMMAND},
	{CMD_CHAIN_GAINS,     sizeof(ChainGains_t),       "CMD_CHAIN_GAINS"    ,        CMD_COMMAND},
	{CMD_ORBIT_VALUES,    sizeof(OrbitValues_t),      "CMD_ORBIT_VALUES"   ,        CMD_COMMAND},
	{CMD_START,                 0,                          "CMD_START"                   ,        CMD_COMMAND},
	{CMD_STOP,                 0,                          "CMD_STOP"                   ,        CMD_COMMAND},
	{CMD_TARGET_STATUS,                 sizeof(TargetStatus_t),                          "CMD_TARGET_STATUS"                   ,        CMD_DATA_STREAM},

	{CMD_SYS_MSG,         0,                          "CMD_SYS_MSG"        ,        CMD_NO_TYPE}, // 0xA0
	{CMD_CAPABILITY,      CMD_DYNAMIC_SIZE,           "CMD_CAPABILITY"     ,    CMD_DATA_STREAM},
	{CMD_STREAM_REQ,      sizeof(StreamReq_t),        "CMD_STREAM_REQ"     , CMD_STREAM_REQUEST},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{CMD_GUI,             0,                          "CMD_GUI"            ,        CMD_NO_TYPE}, // 0xB0
	{CMD_GUI_CMD,         0,                          "CMD_GUI_CMD"        ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{CMD_ANT_TRACK,       sizeof(AntPoint_t),         "CMD_ANT_TRACK"      ,        CMD_COMMAND}, // 0xC0
	{CMD_ANT_PAS_TELEM,   sizeof(AntPoint_t),         "CMD_ANT_PAS_TELEM"  ,        CMD_STREAM_REQUEST},
	{CMD_ANT_APS_TELEM,   sizeof(AntPoint_t),         "CMD_ANT_APS_TELEM"  ,        CMD_STREAM_REQUEST},
	{CMD_ANT_SETTINGS,    sizeof(AntSettings_t),      "CMD_ANT_SETTINGS"   ,        CMD_COMMAND},
	{CMD_ANT_STATE,       sizeof(AntState_t),         "CMD_ANT_STATE"      ,        CMD_COMMAND},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{255,                 0,                          ""                   ,        CMD_NO_TYPE}, // 0xD0
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{255,                 0,                          ""                   ,        CMD_NO_TYPE}, // 0xE0
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

	{255,                 0,                          ""                   ,        CMD_NO_TYPE}, // 0xF0
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},
	{255,                 0,                          ""                   ,        CMD_NO_TYPE},

};


//////////////////////////////////////////////////////////////////////
// The Command Class
//////////////////////////////////////////////////////////////////////

//----------------------------------------------------------
// Constructors - Destructors
//----------------------------------------------------------
Command::Command(const Command &cmd)
{
	length = cmd.size();
	command = new uint8_t[length];

	memcpy(command, cmd.command, length);
}

Command::Command(int size)
{
	command = NULL;

	if(size > 0) {
		command = new uint8_t[size];
		(void)memset((char *)command, 0, size);
	}
	length = size;
}

Command::~Command()
{
	if( command != NULL)
		delete [] command;
	command = NULL;
}

// If you use this, you must check the csum to make sure the data
// in the buffer was valid
Command::Command(const uint8_t * const buf)
{
	command = NULL; 
	length = 0;
	copyFromBuf(buf);
}	

Command::Command(CMDVECTOR &cmd)
{
	// don't assume that the length of the vector
	// is the same as the command, so get the length
	// from whats in the vector
	length = CMD_SIZE(cmd);

	// create new command 
	command = new uint8_t[length];
	assert(command != NULL);

	for(int i=0; i < length; i++)
		command[i] = cmd[i];
}	

//----------------------------------------------------------
// Buffer operations and functions
// The user must guaranty the size of buffer.
// If you use this, you must check the csum to make sure the data
// in the buffer was valid.
//
// -- Since Jack decided not to follow my rules, I need to check
// the buffere for a valid command
//----------------------------------------------------------
void Command::copyFromBuf(const uint8_t * const buf)
{
	// empty/create new command 
	if(command != NULL)
		delete [] command;
	command = NULL;

	length = CMD_SIZE(buf);
	if( length >= CMD_PACKET_SIZE && length < CMD_MAX_SIZE ) {
		if( C_calcCheckSum((uint8_t *)buf) == 0 ) {
			command = new uint8_t[length];
			assert(command != NULL);
			(void)memcpy(command, buf, length);
		} else {
			printf("command::copyFromBuf -- invalid checksum of command in buffer\n");
			length = 0;
		}
	} else {
		printf("command::copyFromBuf -- invalid length of command in buffer\n");
		length = 0;
	}
}   

void Command::copyToBuf(const uint8_t * buf) const
{
	if(buf == NULL || command == NULL)
		return;

	memcpy((char *)buf, command, length);
}  

//----------------------------------------------------------
// Checksum functions
//----------------------------------------------------------
uint16_t Command::calcCheckSum() const
{
	register uint32_t sum = 0;

	// sum up all bytes
	for(uint16_t i=0; i < length - CMD_CSUM_SIZE; i++){
		sum += command[i];
	}
	sum += CMD_CSUM(command);	

	//  Fold 32-bit sum to 16 bits
	while (sum>>16)
		sum = (sum & 0xffff) + (sum >> 16);

	return ~sum;
}

uint16_t C_calcCheckSum(uint8_t* command)
{
	if( !command ) return 0;
	register uint32_t sum = 0;
	uint16_t length =  CMD_SIZE(command) - CMD_CSUM_SIZE;

	// sum up all bytes
	for(uint16_t i=0; i < length; i++)
		sum += command[i];
	sum += CMD_CSUM(command);       

	//  Fold 32-bit sum to 16 bits
	while (sum>>16)
		sum = (sum & 0xffff) + (sum >> 16);

	return ~sum;
}


//----------------------------------------------------------
// access the cmd field
//----------------------------------------------------------
uint8_t Command::cmd() const { 
	assert( length >= CMD_PACKET_SIZE );
	return CMD_CMD(command); 
}         

void Command::cmd(uint8_t cmdId) {
	assert( length >= CMD_PACKET_SIZE );
	CMD_CMD(command) = cmdId; 
}         

//----------------------------------------------------------
// access the seq field
// When setting seq, this is a special case as the user 
// should never set this directly, but the interface that sends
// the command should set this
//----------------------------------------------------------
uint8_t Command::seq() const {
	assert( length >= CMD_PACKET_SIZE );
	return CMD_SEQ(command);
}
void Command::seq(uint8_t seqNo) {
	assert( length >= CMD_PACKET_SIZE );
	CMD_SEQ(command) = seqNo;
}

//----------------------------------------------------------
// access the num field
// When setting num, this is a special case as the user 
// might expect the length to be expanded to fit the number
// of parameters given.
//----------------------------------------------------------
uint16_t Command::num() const {
	assert( length >= CMD_PACKET_SIZE );

	if( length < CMD_PACKET_SIZE )
		printf("Command::num() -- invalid length\n");

	return CMD_NUM(command);
}
void Command::num(uint16_t newNum) {
	assert( length >= CMD_PACKET_SIZE );
	assert( command );

	if( newNum > 0 && (length - newNum) < CMD_PACKET_SIZE ) {
		// keep old bytes and values
		Command temp(command);

		delete [] command;
		command = NULL;

		command = new uint8_t[CMD_PACKET_SIZE + newNum];

		temp.copyToBuf( command );
		length = CMD_PACKET_SIZE + newNum;
	}

	CMD_NUM(command) = newNum;
}

//----------------------------------------------------------
// access the param field (which is an array)
//----------------------------------------------------------
uint8_t Command::param(uint8_t num) const {
	assert( length > CMD_PACKET_SIZE && num < length - CMD_PACKET_SIZE );
	return CMD_PARAM(command,num);
}
uint8_t Command::param(uint8_t num, uint8_t p) {
	assert( length > CMD_PACKET_SIZE && length - CMD_PACKET_SIZE > num );
	CMD_PARAM(command,num) = p;
	return p;
}
void Command::getParams(uint8_t * const buf) const {
	for(int i=0; i < CMD_NUM(command) && i < CMD_MAX_PARAM; i++)
		buf[i] = CMD_PARAM(command,i);
}
void Command::setParams(const uint8_t * const buf, int length) {
	for(int i=0; i < length && i < CMD_MAX_PARAM; i++)
		CMD_PARAM(command,i) = buf[i];
}

//----------------------------------------------------------
// access the csum field
//----------------------------------------------------------
uint16_t Command::csum() const{
	assert( length >= CMD_PACKET_SIZE );

	return CMD_CSUM(command);
}

void Command::setCheckSum()  {
	assert( length >= CMD_PACKET_SIZE ) ;

	CMD_CSUM(command) = 0;
	CMD_CSUM(command) = calcCheckSum();
}

//----------------------------------------------------------
// 
//----------------------------------------------------------
int Command::size() const {
	return length;
}

bool Command::isValid() const { 
	if( length < CMD_PACKET_SIZE )
		return false;

	if( !isDefined() )
		return false;

	return ( calcCheckSum() == 0 ? true : false);
}

bool Command::isDefined() const { 
	if( DefinedCommands[ cmd() ].id == cmd() )  
		if ( DefinedCommands[ cmd() ].numParams == num() 
		  || DefinedCommands[ cmd() ].numParams == CMD_DYNAMIC_SIZE )
			return true;
	return false;
}

//----------------------------------------------------------
// output functions
//----------------------------------------------------------
ostream& operator << (ostream& ostrm, const Command& cmd) {
	ostrm << "____________________ Command ____________________" << endl
		<< " Command: 0x" << hex << (int)cmd.cmd() << dec
		<< " Seq #: " << (int)cmd.seq()
		<< " Bytes: " << (int)cmd.num() << endl;

	for(int i=0; i < (int) cmd.num(); i++){
		if(i!=0 && i%10 == 0)
			ostrm << endl;

		ostrm << "\t" << hex << (int) cmd.param(i);
	}

	ostrm << hex << "\n Check-sum : 0x" << cmd.csum()
		<< " Calculated: 0x" << cmd.calcCheckSum() << endl
		<< "_________________________________________________\n\n"
		<< flush << dec;

	return ostrm;
}

void Command::printCmd() const
{
	cout << *this;
}

//----------------------------------------------------------
// Assignment operators
//----------------------------------------------------------
Command& Command::operator = (const CMDVECTOR &cmd) {
	// delete old command
	if(command != NULL)
		delete [] command;
	command = NULL;

	// don't assume that the length of the vector
	// is the same as the command, so get the length
	// from whats in the vector
	length = CMD_SIZE(cmd);
	command = new uint8_t[length];

	for(int i=0; i < length; i++)
		command[i] = cmd[i];

	return *this;
}

Command& Command::operator = (const Command& cmd)
{
	if(command != NULL)
		delete [] command;
	command = NULL;

	length = cmd.size();
	command = new uint8_t[length];

	memcpy(command, cmd.command, length);
	return *this;
}

Command& Command::operator = (const uint8_t *buf)
{
	copyFromBuf(buf);
	return *this;
}

//----------------------------------------------------------
// equality operators
//----------------------------------------------------------
bool Command::operator == (const Command& cmd) const
{
	if( this->length == cmd.length )
		for(int i = 0; i < length; i++)
			if( command[i] != cmd.command[i] )
				return false ;

	return true;
}

bool Command::operator != (const Command& cmd) const
{
	if( this->length == cmd.length )
	{
		for(int i = 0; i < length; i++)
			if( command[i] != cmd.command[i] )
				return true ;

		return false;
	}    

	return true;
}    

CommandType_t Command::getType() const
{
  if( cmd() > 0 && cmd() < 255 )
		return DefinedCommands[ cmd() ].type;
	return CMD_NO_TYPE;
}

const char * const Command::getName() const
{
  if( cmd() > 0 && cmd() < 255 )
		return DefinedCommands[ cmd() ].name;
	else
		return NULL;
}

//////////////////////////////////////////////////////////////////////
// The CommandQueue Class
//////////////////////////////////////////////////////////////////////

// private functions
uint16_t CommandQueue::calcCheckSum() const 
{
	register uint32_t sum = 0;
	int length = CMD_SIZE(fifo);
	//cout << "command size: " << CMD_SIZE(fifo) << endl;

	// sum up all bytes
	for(int i=0; i < length - CMD_CSUM_SIZE; i++){
		sum += fifo[i];
		//printf("[%x]",fifo[i]);  
	}
	//cout << endl;

	uint32_t mysum = sum;
	while (mysum>>16)
		mysum = (mysum & 0xffff) + (mysum >> 16);

	//printf("checksum: 0x%x\n",~mysum & 0xffff);

	uint8_t val[2];
	val[0] = fifo[ CMD_NUM(fifo) + CMD_HEADER_SIZE ];
	val[1] = fifo[ CMD_NUM(fifo) + CMD_HEADER_SIZE + 1 ];

	sum += *( (uint16_t *)(&val) );

	//  Fold 32-bit sum to 16 bits
	while (sum>>16)
		sum = (sum & 0xffff) + (sum >> 16);

	return ~sum;
}

// append to the queue, a buffer of length num
void CommandQueue::push(uint8_t * buf, size_t num) 
{
	for(register size_t i=0; i < num ; i++)
		fifo.push_back( buf[i] );
}

// pop a single byte off of the queue
uint8_t CommandQueue::popByte() 
{
	assert( ! fifo.empty() );

	uint8_t val = fifo.front();
	fifo.erase(fifo.begin());
	return val;
}

// pop a single byte off of the queue
void CommandQueue::pop() 
{
	if( fifo.empty() )
		return;

	fifo.erase(fifo.begin());
}

// pop a buffer of maximum length bytes off of the queue
// returns the number actual popped off
int CommandQueue::popBuf(char *buf, size_t length) 
{
	if( fifo.empty() )
		return -1;

	size_t num=0; // number actually popped

	for( num=0; num< fifo.size() && num<length; num++ )
		buf[num] = fifo[num];

	fifo.erase( fifo.begin(), fifo.begin() + num);

	return (int)num;
}

// pop a command and move down the received bytes by the
// size of the command
Command CommandQueue::popCommand() 
{
	assert( ! fifo.empty() );

	Command cmd(fifo);
	fifo.erase( fifo.begin(), fifo.begin() + cmd.size() );
	return cmd;
}

// pop a command and move down the received bytes by the
// size of the command
Command CommandQueue::peekCommand() {
	assert( ! fifo.empty() );

	Command cmd(fifo);
	return cmd;
}

// checks to see if the next set of bytes on the byte
// queue form a valid command
	bool CommandQueue::isValid() const {
		if( fifo.size() < (size_t)CMD_SIZE(fifo) )
			return false;

		return ( calcCheckSum() == 0 ? true : false);
	}

bool CommandQueue::isDefined() const 
{ 
	if( DefinedCommands[ CMD_CMD(fifo) ].id == CMD_CMD(fifo) )	
		if ( DefinedCommands[ CMD_CMD(fifo) ].numParams == CMD_NUM(fifo) || 
				DefinedCommands[ CMD_CMD(fifo) ].numParams == 255 ||
				CMD_NUM(fifo) == 0)
			return true;

	// output debugging 
	/*cout << hex 
	  << "CMD_CMD(fifo)=" << (int)CMD_CMD(fifo)
	  << " DefinedCommands[].id=" << (int)DefinedCommands[ CMD_CMD(fifo) ].id 
	  << endl
	  << "CMD_NUM(fifo)=" << (int)CMD_NUM(fifo)
	  << " DefinedCommands[].numParams=" << (int)DefinedCommands[ CMD_CMD(fifo) ].numParams 
	  << dec << endl;
	  */

	return false;
}

// check to see if there are enough bytes to make 
// the command 
bool CommandQueue::needMoreBytes() const
{
	if( fifo.size() < (size_t)CMD_SIZE(fifo) )
		return true;
	return false;
}


//------------------------------------------------------------ 
// commandStruct functions and data below
//
// See commandStruct.h for documentation
//-------------------------------------------------------------------- 
float char2float(char val) {
	return (float)(val/127.0);
}   
char float2char(float val) {
	return (char)(val*127);
}
float uchar2float(uint8_t val) {
	return (float)( (val - 127)/127.0);
}   
float uchar2ufloat(uint8_t val) {
	return (float)( (val)/254.0);
}   
uint8_t float2uchar(float val) {
	return (uint8_t)( (val + 1)/2.0 * 254);
}
